#ifndef FAKE_ODOMETRY_ROS_H
#define FAKE_ODOMETRY_ROS_H

#include <rclcpp/rclcpp.hpp>
#include <geometry_msgs/msg/twist.hpp>
#include <geometry_msgs/msg/pose_with_covariance_stamped.hpp>
#include <agv_msgs/msg/odometry_lite.hpp>
#include "ros_utils/TransformHandler.h"
#include "ros_utils/transform.h"
#include "fake_odom/Robot.h"

class FakeOdometry : public rclcpp::Node
{
public:
  FakeOdometry();
  bool init();
  
private:
  // Callbacks
  void cmdVelCallback(const geometry_msgs::msg::Twist::SharedPtr msg);
  void deadReckoning();
  void setInitialPose(const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg);
  
  // Helper functions
  void toOdomMsg(const Eigen::Vector3d& pose_2d, agv_msgs::msg::OdometryLite& msg);
  void toTransformMsg(const Eigen::Vector3d& pose_2d, geometry_msgs::msg::Transform& transform);
  void initPose();
  void backUpPose();

  // Transform handler
  std::shared_ptr<utils::TransformHandler> tf_;

  // Topics
  std::string cmdvel_topic_;
  std::string initialpose_topic_;
  std::string odom_topic_;

  // Frame IDs
  std::string baselink_frame_;
  std::string odom_frame_;
  std::string map_frame_;

  // Timer Options
  double odom_pub_rate_;

  // TF Broadcaster Options
  bool enable_map2odom_;

  // Subscribers
  rclcpp::Subscription<geometry_msgs::msg::Twist>::SharedPtr sub_cmdvel_;
  rclcpp::Subscription<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr sub_initialpose_;
  
  // Publishers
  rclcpp::Publisher<agv_msgs::msg::OdometryLite>::SharedPtr pub_odom_;
  
  // Timers
  rclcpp::TimerBase::SharedPtr pub_odom_timer_;

  // Robot state
  Robot robot_;
  geometry_msgs::msg::Twist cmd_vel_;
  
  // Noise parameters
  double static_noise_level_;
  double move_noise_level_;
  double yaw_noise_level_;
};

#endif  // FAKE_ODOMETRY_ROS_H